#include "kinco.hpp"
#include <iomanip>
#include <bitset>
#include <chrono>
#include <ctime>
using namespace CANalyst2;

// 定义错误码映射表
static const std::map<uint16_t, std::string> emcy_error_map = {
    {0x7331, "Encoder communication error (编码器未连接或通讯超时)"},
    {0x7320, "Encoder internal fault (编码器内部故障)"},
    {0x7330, "Encoder CRC error (编码器通讯受到干扰)"},
    {0x4210, "Drive over temperature (驱动器温度过高)"},
    {0x3210, "Bus voltage too high (总线电压过高)"},
    {0x3220, "Bus voltage too low (总线电压过低)"},
    {0x2320, "Drive output short circuit (驱动器功率管或电机短路)"},
    {0x7110, "Brake resistor fault (制动电阻过载)"},
    {0x8611, "Following error exceeds limit (实际跟随误差超过允许)"},
    {0x5112, "Logic voltage low (逻辑电源电压过低)"},
    {0x2350, "Motor or drive IIt fault (电机或驱动器 IIt 故障)"},
    {0x8A80, "Input pulse frequency too high (输入脉冲频率过高)"},
    {0x4310, "Motor over temperature (电机温度过高)"},
    {0x6310, "EEPROM data error (EEPROM 数据校验错误)"},
    {0x5210, "Current sensor fault (电流传感器故障)"},
    {0x6010, "Watchdog error (软件看门狗异常)"},
    {0x6011, "Exception interrupt (异常中断)"},
    {0x7400, "MCU fault (MCU 型号错误)"},
    {0x6320, "Motor config error (电机未配置)"},
    {0x5443, "Pre-enable warning (预使能报警)"},
    {0x5442, "Positive limit error (正限位报错)"},
    {0x5441, "Negative limit error (负限位报错)"},
    {0x6012, "SPI fault (SPI 故障)"},
    {0x8100, "CAN bus interruption (CAN 总线中断)"},
    {0x8A81, "Full closed-loop fault (全闭环故障)"},
    {0x7382, "Main encoder ABZ fault (主编码器 ABZ 故障)"},
    {0x7306, "Main encoder index error (主编码器计数错误)"},
    {0x7111, "Brake resistor over temp (制动电阻过温)"},
    {0x7112, "Brake resistor short circuit (制动电阻短路)"},
    {0x6321, "Motor phase missing (电机缺相)"},
    {0x2321, "ADC saturation (电流采样 ADC 饱和)"},
    {0x81FF, "Service timeout (通讯总线错误扩展)"}
};

Kinco::Kinco(std::shared_ptr<CANDispatch> _can, int ch, int _slave_id, bool _is_pos_mode, int _resolution, 
                uint32_t _profiled_vel, uint32_t _profiled_acc, uint32_t _profiled_dec) : 
        is_pos_mode(_is_pos_mode), can(_can), resolution(_resolution), 
        profiled_vel(_profiled_vel), profiled_acc(_profiled_acc), profiled_dec(_profiled_dec)
{
    slave_id = _slave_id;
    can_ch = ch;
    _is_fault = false;
    _error_code = 0;
    _error_req = 0;
    _error_cnt = 0;
    is_motor_on = false;
    memset(&status_fbk, 0, sizeof(status_fbk));

    auto timeout = [this]()
    {
        std::cout << "Kinco servo timeout,addr = " << int(slave_id) << std::endl;
        _is_fault = true;
        _error_code = 0xFFFF;
    };

    can->Register(
        0x180 + slave_id, [this](const struct CanStamped &frame)->void
        { this->processRecvTPDOFrame(frame); },
        200,
        timeout);

    can->Register(
        0x80 + slave_id, [this](const struct CanStamped &frame)->void
        { this->processRecvEmcyFrame(frame); },
        200,
        timeout);

    can->Register(
        0x580 + slave_id, [this](const struct CanStamped &frame)->void
        // {this->updateFault(frame);}
        {},
        200,
        timeout);
}

Kinco::~Kinco()
{
    std::cout << "~Kinco() is called" << std::endl;
    // Disable();
    GetCurStatusPDO();
    if(current_status != Servo_Status::SWITCH_ON_DISABLED) {
        SendDisableMotorPDO();
    }
}

int Kinco::SendEnableMotorPDO()
{
    can->clr_send_buffer(can_ch);
    for(int i = 0; i < 10; i++)
    {
        GetCurStatusPDO();
        if(current_status != Servo_Status::INVALID) {
            break;
        }
    }

    if(current_status == Servo_Status::INVALID) {
        std::cout << "Servo motor enable failed, cann't acquire servo motor status-word." << std::endl;
        return -1;
    }
    else if(current_status == Servo_Status::SWITCH_ON_DISABLED) {
        std::cout << "Current status is SWITCH_ON_DISABLED, continue..." << std::endl;
    }

    CanStamped send_frame;
    send_frame.ID = 0x201;
    send_frame.DataLen = 8;
    send_frame.Data[0] = 0x06;
    send_frame.Data[1] = 0x00;
    send_frame.Data[2] = 0x00;
    send_frame.Data[3] = 0x00;
    send_frame.Data[4] = 0x00;
    send_frame.Data[5] = 0x00;
    send_frame.Data[6] = 0x00;
    send_frame.Data[7] = 0x00;

    for(int i = 0; i < 10; i++)
    {
        can->clr_send_buffer(can_ch);
        can->Write(can_ch, send_frame);
        can->send_msg(can_ch);
        usleep(500000);
        SendSyncFrame();
        can->recv_msg(can_ch);
        if(current_status == Servo_Status::READY_TO_SWITCH_ON) {
            break;
        }
    }

    if(current_status != Servo_Status::READY_TO_SWITCH_ON) {
        std::cout << "Servo motor enable failed at SWITCH_ON_DISABLED to READY_TO_SWITCH_ON." << std::endl;
        return -1;
    }
    else {
        std::cout << "Current status is READY_TO_SWITCH_ON, continue..." << std::endl;
    }

    send_frame.Data[0] = 0x07;

    for(int i = 0; i < 10; i++)
    {
        can->Write(can_ch, send_frame);
        can->send_msg(can_ch);
        usleep(500000);
        SendSyncFrame();
        can->recv_msg(can_ch);
        if(current_status == Servo_Status::SWITCHED_ON) {
            break;
        }
    } 
    
    if(current_status != Servo_Status::SWITCHED_ON) {
        std::cout << "Servo motor enable failed at READY_TO_SWITCH_ON to SWITCHED_ON." << std::endl;
        return -1;
    }
    else {
        std::cout << "Current status is SWITCHED_ON, continue..." << std::endl;
    }

    for(int i = 0; i < 10; i++) {
        send_frame.Data[0] = 0x0F;
        can->Write(can_ch, send_frame);
        can->send_msg(can_ch);
        usleep(500000);
        SendSyncFrame();
        can->recv_msg(can_ch);
        if(current_status == Servo_Status::OPERATION_ENABLED) {
            break;
        }
    }

    if(current_status != Servo_Status::OPERATION_ENABLED) {
        std::cout << "Servo motor enable failed at SWITCHED_ON to OPERATION_ENABLED." << std::endl;
        return -1;
    }
    else {
        std::cout << "Current status is OPERATION_ENABLED, enable completed." << std::endl;
    }

    return 0;
}

int Kinco::SendDisableMotorPDO()
{
    can->clr_send_buffer(can_ch);

    CanStamped send_frame;
    send_frame.ID = 0x201;
    send_frame.DataLen = 8;
    send_frame.Data[0] = 0x06;
    send_frame.Data[1] = 0x00;
    send_frame.Data[2] = 0x00;
    send_frame.Data[3] = 0x00;
    send_frame.Data[4] = 0x00;
    send_frame.Data[5] = 0x00;
    send_frame.Data[6] = 0x00;
    send_frame.Data[7] = 0x00;

    for(int i = 0; i < 10; i++)
    {
        can->clr_send_buffer(can_ch);
        can->Write(can_ch, send_frame);
        can->send_msg(can_ch);
        usleep(500000);
        SendSyncFrame();
        can->recv_msg(can_ch);
        if(current_status == Servo_Status::READY_TO_SWITCH_ON) {
            break;
        }
    }

    if(current_status != Servo_Status::READY_TO_SWITCH_ON) {
        std::cout << "Servo motor disabled failed at OPERATION_ENABLED to READY_TO_SWITCH_ON." << std::endl;
        return -1;
    }

    send_frame.Data[0] = 0x00;
    for(int i = 0; i < 10; i++)
    {
        can->Write(can_ch, send_frame);
        can->send_msg(can_ch);
        usleep(500000);
        SendSyncFrame();
        can->recv_msg(can_ch);
        if(current_status == Servo_Status::SWITCH_ON_DISABLED) {
            break;
        }
    } 

    if(current_status != Servo_Status::SWITCH_ON_DISABLED) {
        std::cout << "Servo motor disabled failed at READY_TO_SWITCH_ON to SWITCH_ON_DISABLED." << std::endl;
        return -1;
    }
    return 0;
}

void Kinco::Setup()
{
    can->clr_send_buffer(can_ch);
    CanStamped setup_frame;
    // COB-ID 0x00,NMT frame
    // ctrl word:0x01 start remote node
    // ctrl word:0x02 close remote node
    setup_frame.ID = 0x00;
    setup_frame.DataLen = 2;
    setup_frame.Data[0] = 0x02;
    setup_frame.Data[1] = slave_id;

    std::cout << "ctrl word:0x02 stop remote node" << std::endl;
    can->Write(can_ch, setup_frame);
    can->Update(can_ch);

    // ctrl word:0x82 reset communication
    std::cout << "ctrl word:0x82 reset communication" << std::endl;
    setup_frame.Data[0] = 0x82;
    setup_frame.Data[1] = 0x00;
    can->Write(can_ch, setup_frame);
    can->Update(can_ch);

    // ctrl word:0x80 enter pre-operational
    std::cout << "ctrl word:0x80 enter pre-operational" << std::endl;
    setup_frame.Data[0] = 0x80;
    setup_frame.Data[1] = slave_id;
    can->Write(can_ch,setup_frame);
    can->Update(can_ch);

    int result = 0;
    if(is_pos_mode == false) {
        // set velocity mode
        result = can->COE().write<uint8_t>(can_ch, slave_id, SERVO_WORK_MODE_INDEX, 0, 3);
    }
    else {
        // set position mode
        result = can->COE().write<uint8_t>(can_ch, slave_id, SERVO_WORK_MODE_INDEX, 0, 1);
    }

    if(result != 0) {
        std::cout << "Setup Kinco servo failed!" << std::endl;
        return;
    } 
    
    SendSetProfiledVelocitySDO(profiled_vel);
    SendSetProfiledAccSDO(profiled_acc);
    SendSetProfiledDecSDO(profiled_dec);
    // can->COE().write<uint32_t>(slave_id, 0x6081, 0, 0x00369D03); // set profiled speed 200RPM
    // can->COE().write<uint32_t>(slave_id, 0x6083, 0, 0x0001A36E); // set profiled acc 100rps/s
    // can->COE().write<uint32_t>(slave_id, 0x6084, 0, 0x0001A36E); // set profiled dec 100rps/s

    // set RPDO1:ctrl_word + target_pos
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1400, 1, (0x8000020 + slave_id)); // disable RPDO1
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1400, 2, 0x01); // sync type
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1600, 0, 0); // clear mapping
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1600, 1, 0x60400010); // mapping 1st parameter is Control Word(index=0x6040, sub-index = 0x10) 
    if (is_pos_mode == false) {
        can->COE().write<uint32_t>(can_ch, slave_id, 0x1600, 2, 0x60FF0020); // mapping 2nd parameter is target velocity
    }
    else {
        can->COE().write<uint32_t>(can_ch, slave_id, 0x1600, 2, 0x607A0020); // mapping 2nd parameter is target position
    }
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1600, 0, 2); // mapping two parameters
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1400, 1, (0x200 + slave_id)); // enable mapping
    
    // set RPDO2:profiled_velocity
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1401, 1, (0x8000021 + slave_id)); // disable RPDO2
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1401, 2, 0x01); // sync type
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1601, 0, 0); // clear mapping
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1601, 1, 0x60810020); // profiled velocity 32bit
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1601, 0, 1); // valid item = 1
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1401, 1, 0x201 + slave_id); // enable RPDO2

    // set TPDO1
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1800, 1, (0x80000180 + slave_id)); // disable TPDO1
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1800, 2, 0x01); // sync type
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1A00, 0, 0); // clear mapping
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1A00, 1, 0x60410010); // mapping 1st parameter is Status Word
    if (is_pos_mode == false) {
        can->COE().write<uint32_t>(can_ch, slave_id, 0x1A00, 2, 0x606c0020); // mapping 2nd parameter is actual velocity RPM
    }
    else {
        can->COE().write<uint32_t>(can_ch, slave_id, 0x1A00, 2, 0x60630020); // mapping 2nd parameter is actual position
    }   
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1A00, 3, 0x60780010); // mapping 3rd parameter is actual current

    can->COE().write<uint8_t>(can_ch, slave_id, 0x1A00, 0, 0x03); // mapping three parameters
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1800, 1, (0x00000180 + slave_id)); // enable mapping

    // start node
    setup_frame.ID = 0x00;
    setup_frame.DataLen = 2;
    setup_frame.Data[0] = 0x01;
    setup_frame.Data[1] = 0x00;
    can->Write(can_ch, setup_frame);
    can->Update(can_ch);
}

void Kinco::SendMoveHomeSDO()
{
    for(int i = 0; i < 10; i++) {
        if(GetCurStatusPDO() != Servo_Status::OPERATION_ENABLED) {
            SendEnableMotorPDO();
        }
        else {
            break;
        }   
    }
    
    if(GetCurStatusPDO() != Servo_Status::OPERATION_ENABLED) {
        std::cout << "Servo motor not in OPERATION_ENABLED, move home failed" << std::endl;
        return;
    }

    SendSetHomeModeSDO();

    is_pos_mode = false;

    can->COE().write<uint8_t>(can_ch, slave_id, 0x6098, 0, 0x21);
    usleep(10000);
    can->COE().write<uint32_t>(can_ch, slave_id, 0x6099, 1, 0x00369D03); // 转折速度 200RPM
    usleep(10000);
    can->COE().write<uint32_t>(can_ch, slave_id, 0x6099, 2, 0x001B4E82); // 搜索速度 100RPM
    usleep(10000);
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x001F);
}

int32_t Kinco::SendMoveTargetAbsSDO(float pos)
{
    for(int i = 0; i < 10; i++) {
        if(GetCurStatusPDO() != Servo_Status::OPERATION_ENABLED) {
            SendEnableMotorPDO();
        }
        else {
            break;
        }   
    }
    
    if(GetCurStatusPDO() != Servo_Status::OPERATION_ENABLED) {
        std::cout << "Servo motor not in OPERATION_ENABLED, move target pos failed" << std::endl;
        return -1; 
    }

    if (is_pos_mode != true) {
        int8_t read_mode = 0;
        bool is_acq = false;
        for (int i = 0; i < 3; i++)
        {
            SendSetWorkModeSDO(Servo_Mode::POS_MODE);
            usleep(10000);
            SendAcquireValidWorkModeSDO(read_mode, is_acq);
            if((read_mode == static_cast<int8_t>(Servo_Mode::POS_MODE)) && is_acq == true) {
                is_pos_mode = true;
                break;
            }
        }
    }

    if(is_pos_mode != true) {
        std::cout << "Set servo mode to pos failed." << std::endl;
        return -1;
    }
    
    can->COE().write<uint32_t>(can_ch, slave_id, TARGET_POS_INDEX, 0, pos); // set target position
    usleep(5000);
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x002F); // trigger move
    usleep(5000);
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x003F); // trigger move
    usleep(5000);
    return 0;
}

int32_t Kinco::SendMoveTargetRelSDO(float pos)
{
    for(int i = 0; i < 10; i++) {
        if(GetCurStatusPDO() != Servo_Status::OPERATION_ENABLED) {
            SendEnableMotorPDO();
        }
        else {
            break;
        }   
    }
    
    if(GetCurStatusPDO() != Servo_Status::OPERATION_ENABLED) {
        std::cout << "Servo motor not in OPERATION_ENABLED, move target pos failed" << std::endl;
        return -1; 
    }

    if (is_pos_mode != true) {
        int8_t read_mode = 0;
        bool is_acq = false;
        for (int i = 0; i < 3; i++)
        {
            SendSetWorkModeSDO(Servo_Mode::POS_MODE);
            usleep(10000);
            SendAcquireValidWorkModeSDO(read_mode, is_acq);
            if((read_mode == static_cast<int8_t>(Servo_Mode::POS_MODE)) && is_acq == true) {
                is_pos_mode = true;
                break;
            }
        }
    }

    if(is_pos_mode != true) {
        std::cout << "Set servo mode to pos failed." << std::endl;
        return -1;
    }

    can->COE().write<uint32_t>(can_ch, slave_id, TARGET_POS_INDEX, 0, pos); // set target position
    usleep(10000);
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x004F); // trigger move
    usleep(10000);
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x005F); // trigger move
    usleep(10000);
    return 0;
}

bool Kinco::SendSetWorkModeSDO(Servo_Mode mode)
{
    bool is_set_ok = false;
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x0F); // Enable Operation
    usleep(10000);
    uint16_t ctrl = 0;
    bool is_acq = false;
    SendAcquireCtrlWordSDO(ctrl, is_acq);
    if (is_acq != true) {
        std::cout << "Set ctrl word to 0xF failed." << std::endl;
        return false;
    }
    
    if(ctrl != 0x0F) {
        std::cout << "Set ctrl word to 0xF failed." << std::endl;
        return false;
    }

    can->COE().write<int8_t>(can_ch, slave_id, SERVO_WORK_MODE_INDEX, 0, static_cast<int8_t>(mode));
    usleep(10000);
    
    int8_t read_mode = 0;
    is_acq = false;
    SendAcquireServoWorkModeSDO(read_mode, is_acq);
    if (is_acq != true) {
        std::cout << "Set work mode failed." << std::endl;
        return false;
    }
    if (read_mode != static_cast<int8_t>(mode)) {
        std::cout << "Set work mode failed." << std::endl;
        return false;
    }

    read_mode = 0;
    is_acq = false;
    SendAcquireValidWorkModeSDO(read_mode, is_acq);
    if (is_acq != true) {
        std::cout << "Set work mode failed." << std::endl;
        return false;
    }
    if (read_mode != static_cast<int8_t>(mode)) {
        std::cout << "Set work mode failed." << std::endl;
        return false;
    }

    if (read_mode != static_cast<int8_t>(Servo_Mode::POS_MODE)) {
        is_pos_mode = false;
    }

    return true;
}

bool Kinco::SendSetTargetVelocitySDO(int32_t rpm)
{
    bool is_set_ok = false;
    uint32_t set_val = rpm * (resolution * RPM_TO_DEC_MULT);
    std::cout << "set velocity:" << std::hex << set_val << std::endl;
    can->COE().write<uint32_t>(can_ch, slave_id, TARGET_VELOCITY_INDEX, 0, set_val); 
    usleep(10000);

    int32_t target_vel = 0;
    bool is_acq = false;
    SendAcquireTargetVelocitySDO(target_vel, is_acq);
    if (is_acq != true) {
        std::cout << "Set target velocity failed." << std::endl;
        return false;
    }
    
    if(target_vel != set_val) {
        std::cout << "Set target velocity failed." << std::endl;
        return false;
    }

    return true;
}

bool Kinco::SendSetProfiledVelocitySDO(uint32_t rpm)
{
    bool is_set_ok = false;
    uint32_t set_val = rpm * (resolution * RPM_TO_DEC_MULT);
    // std::cout << "resolution:" << resolution << "set velocity:" << std::hex << set_val << std::endl;
    can->COE().write<uint32_t>(can_ch, slave_id, PROFILED_VELOCITY_INDEX,  0, set_val); 
    usleep(10000);

    uint32_t vel = 0;
    bool is_acq = false;
    SendAcquireProfiledVelSDO(vel, is_acq);
    if (is_acq != true) {
        std::cout << "Set profiled velocity failed." << std::endl;
        return false;
    }
    
    if(vel != set_val) {
        std::cout << "Set profiled velocity failed." << std::endl;
        return false;
    }

    return true;
}

bool Kinco::SendSetProfiledVelocityPDO(uint32_t rpm)
{
    uint32_t set_val = rpm * (resolution * RPM_TO_DEC_MULT);

    can->clr_send_buffer(can_ch);
    CanStamped send_frame;
    send_frame.ID = 0x201 + slave_id;
    send_frame.DataLen = 8;

    send_frame.Data[0] = static_cast<uint8_t> (set_val & 0xFF);
    send_frame.Data[1] = static_cast<uint8_t> ((set_val >> 8) & 0xFF);
    send_frame.Data[2] = static_cast<uint8_t> ((set_val >> 16) & 0xFF);
    send_frame.Data[3] = static_cast<uint8_t> ((set_val >> 24) & 0xFF);
    send_frame.Data[4] = 0x00;
    send_frame.Data[5] = 0x00;
    send_frame.Data[6] = 0x00;
    send_frame.Data[7] = 0x00;
    can->Write(can_ch, send_frame);
    can->send_msg(can_ch);
}

bool Kinco::SendSetProfiledAccSDO(uint32_t rps)
{
    bool is_set_ok = false;
    uint32_t set_val = resolution * rps * ACC_DEC_MULT;
    can->COE().write<uint32_t>(can_ch, slave_id, PROFILED_ACC_INDEX,  0, set_val); 
    usleep(10000);

    uint32_t vel = 0;
    bool is_acq = false;
    SendAcquireProfiledAccSDO(vel, is_acq);
    if (is_acq != true) {
        std::cout << "Set profiled acceleration failed." << std::endl;
        return false;
    }
    
    if(vel != set_val) {
        std::cout << "Set profiled acceleration failed." << std::endl;
        return false;
    }

    return true;
}

bool Kinco::SendSetProfiledDecSDO(uint32_t rps)
{
    bool is_set_ok = false;
    uint32_t set_val = resolution * rps * ACC_DEC_MULT;
    can->COE().write<uint32_t>(can_ch, slave_id, PROFILED_DEC_INDEX,  0, set_val); 
    usleep(10000);

    uint32_t vel = 0;
    bool is_acq = false;
    SendAcquireProfiledDecSDO(vel, is_acq);
    if (is_acq != true) {
        std::cout << "Set profiled deceleration failed." << std::endl;
        return false;
    }
    
    if(vel != set_val) {
        std::cout << "Set profiled deceleration failed." << std::endl;
        return false;
    }

    return true;
}

void Kinco::SendAcquireValidWorkModeSDO(int8_t &mode, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, VALID_WORK_MODE_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        mode = can->COE().get_valid_work_mode();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void Kinco::SendAcquireServoWorkModeSDO(int8_t &mode, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, SERVO_WORK_MODE_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        mode = can->COE().get_servo_work_mode();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void Kinco::SendAcquireStatusWordSDO(uint16_t &status, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, SERVO_STATUS_WORD_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        status = can->COE().get_current_status_word();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}


void Kinco::SendAcquireCtrlWordSDO(uint16_t &ctrl, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        ctrl = can->COE().get_current_ctrl_word();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void Kinco::SendAcquireTargetPosSDO(int32_t &pos, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, TARGET_POS_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        pos = can->COE().get_target_pos();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void Kinco::SendAcquireActualPosSDO(int32_t &pos, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, TARGET_POS_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        pos = can->COE().get_actual_pos();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void Kinco::SendAcquireTargetVelocitySDO(int32_t &vel, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, TARGET_VELOCITY_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        vel = can->COE().get_target_velocity();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void Kinco::SendAcquireActualVelocitySDO(int32_t &vel, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, TARGET_VELOCITY_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        vel = can->COE().get_actual_velocity();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void Kinco::SendAcquireProfiledVelSDO(uint32_t &vel, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, PROFILED_VELOCITY_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        vel = can->COE().get_profiled_vel();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void Kinco::SendAcquireProfiledAccSDO(uint32_t &acc, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, PROFILED_ACC_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        acc = can->COE().get_profiled_acc();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void Kinco::SendAcquireProfiledDecSDO(uint32_t &dec, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, PROFILED_DEC_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        dec = can->COE().get_profiled_dec();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void Kinco::SendAcquireActualCurrentSDO(int16_t current, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, ACTUAL_CURRENT_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        current = can->COE().get_actual_current();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void Kinco::SendMoveTargetPosPDO(float pos)
{
    if (is_pos_mode != true) {
        int8_t read_mode = 0;
        bool is_acq = false;
        for (int i = 0; i < 3; i++)
        {
            SendSetWorkModeSDO(Servo_Mode::POS_MODE);
            usleep(10000);
            SendAcquireValidWorkModeSDO(read_mode, is_acq);
            if((read_mode == static_cast<int8_t>(Servo_Mode::POS_MODE)) && is_acq == true) {
                is_pos_mode = true;
                break;
            }
        }
    }

    if(is_pos_mode != true) {
        std::cout << "Set servo mode to pos failed." << std::endl;
        return;
    }

    can->clr_send_buffer(can_ch);
    CanStamped send_frame;
    send_frame.ID = 0x200 + slave_id;
    send_frame.DataLen = 8;

    // ctrl word
    send_frame.Data[0] = 0x2F;
    send_frame.Data[1] = 0x00;

    // transfer float pos to int32_t
    int32_t pos_int = static_cast<int32_t>(pos);
    send_frame.Data[2] = static_cast<uint8_t>(pos_int & 0xFF);
    send_frame.Data[3] = static_cast<uint8_t>((pos_int >> 8) & 0xFF);
    send_frame.Data[4] = static_cast<uint8_t>((pos_int >> 16) & 0xFF);
    send_frame.Data[5] = static_cast<uint8_t>((pos_int >> 24) & 0xFF);
    send_frame.Data[6] = 0x00;
    send_frame.Data[7] = 0x00;
    can->Write(can_ch, send_frame);

    // ctrl word
    send_frame.Data[0] = 0x3F;
    send_frame.Data[1] = 0x00;

    can->Write(can_ch, send_frame);
    can->send_msg(can_ch);
    // usleep(500000);
}

void Kinco::SendMoveTargetPosPDO(float pos, uint32_t rpm)
{
    if (is_pos_mode != true) {
        int8_t read_mode = 0;
        bool is_acq = false;
        for (int i = 0; i < 3; i++)
        {
            SendSetWorkModeSDO(Servo_Mode::POS_MODE);
            usleep(10000);
            SendAcquireValidWorkModeSDO(read_mode, is_acq);
            if((read_mode == static_cast<int8_t>(Servo_Mode::POS_MODE)) && is_acq == true) {
                is_pos_mode = true;
                break;
            }
        }
    }

    if(is_pos_mode != true) {
        std::cout << "Set servo mode to pos failed." << std::endl;
        return;
    }

    uint32_t set_val = rpm * (resolution * RPM_TO_DEC_MULT);

    can->clr_send_buffer(can_ch);
    CanStamped send_frame;
    send_frame.ID = 0x201 + slave_id;
    send_frame.DataLen = 8;

    send_frame.Data[0] = static_cast<uint8_t> (set_val & 0xFF);
    send_frame.Data[1] = static_cast<uint8_t> ((set_val >> 8) & 0xFF);
    send_frame.Data[2] = static_cast<uint8_t> ((set_val >> 16) & 0xFF);
    send_frame.Data[3] = static_cast<uint8_t> ((set_val >> 24) & 0xFF);
    send_frame.Data[4] = 0x00;
    send_frame.Data[5] = 0x00;
    send_frame.Data[6] = 0x00;
    send_frame.Data[7] = 0x00;
    can->Write(can_ch, send_frame);

    send_frame.ID = 0x200 + slave_id;
    // send_frame.DataLen = 8;

    // ctrl word
    send_frame.Data[0] = 0x2F;
    send_frame.Data[1] = 0x00;

    // transfer float pos to int32_t
    int32_t pos_int = static_cast<int32_t>(pos);
    send_frame.Data[2] = static_cast<uint8_t>(pos_int & 0xFF);
    send_frame.Data[3] = static_cast<uint8_t>((pos_int >> 8) & 0xFF);
    send_frame.Data[4] = static_cast<uint8_t>((pos_int >> 16) & 0xFF);
    send_frame.Data[5] = static_cast<uint8_t>((pos_int >> 24) & 0xFF);
    send_frame.Data[6] = 0x00;
    send_frame.Data[7] = 0x00;
    can->Write(can_ch, send_frame);

    // ctrl word
    send_frame.Data[0] = 0x3F;
    send_frame.Data[1] = 0x00;

    can->Write(can_ch, send_frame);
    can->send_msg(can_ch);
    // usleep(500000);
}

void Kinco::parseStatusWord(uint16_t status)
{
    // std::cout << "Statusword: 0x" 
    //           << std::hex << std::setw(4) << std::setfill('0') << status 
    //           << std::dec << " (" << std::bitset<16>(status) << ")\n";

    // CiA 402 标准状态位
    // if (status & (1 << 0)) std::cout << " - Ready to Switch On\n";
    // if (status & (1 << 1)) std::cout << " - Switched On\n";
    // if (status & (1 << 2)) std::cout << " - Operation Enabled\n";
    // if (status & (1 << 3)) std::cout << " - Fault\n";
    // if (status & (1 << 4)) std::cout << " - Voltage Enabled\n";
    // if (status & (1 << 5)) std::cout << " - Quick Stop Active\n";
    // if (status & (1 << 6)) std::cout << " - Switch On Disabled\n";
    // if (status & (1 << 7)) std::cout << " - Warning\n";
    // if (status & (1 << 8)) std::cout << " - Manufacturer specific (bit8)\n";
    // if (status & (1 << 9)) std::cout << " - Remote (Set-point from network)\n";
    // if (status & (1 << 10)) std::cout << " - Target Reached\n";
    // if (status & (1 << 11)) std::cout << " - Internal Limit Active\n";
    // if (status & (1 << 12)) std::cout << " - Manufacturer specific (bit12)\n";
    // if (status & (1 << 13)) std::cout << " - Manufacturer specific (bit13)\n";
    // if (status & (1 << 14)) std::cout << " - Manufacturer specific (bit14)\n";
    // if (status & (1 << 15)) std::cout << " - Manufacturer specific (bit15)\n";

    // 简单判断状态机模式（参考 CiA 402）
    if ((status & 0x004F) == 0x0000) {
        std::cout << "State: Not ready to switch on\n";
    }
    else if ((status & 0x004F) == 0x0040) {
        std::cout << "State: Switch on disabled\n";
        current_status = Servo_Status::SWITCH_ON_DISABLED;
    }
    else if ((status & 0x006F) == 0x0021) {
        std::cout << "State: Ready to switch on\n";
        current_status = Servo_Status::READY_TO_SWITCH_ON;
    }
    else if ((status & 0x006F) == 0x0023) {
        std::cout << "State: Switched on\n";
        current_status = Servo_Status::SWITCHED_ON;
    }
    else if ((status & 0x006F) == 0x0027) {
        std::cout << "State: Operation enabled\n";
        current_status = Servo_Status::OPERATION_ENABLED;
    }
    else if ((status & 0x006F) == 0x0007) {
        std::cout << "State: Quick stop active\n";
    }
    else if ((status & 0x004F) == 0x000F) {
        std::cout << "State: Fault reaction active\n";
    }
    else if ((status & 0x004F) == 0x0008) {
        std::cout << "State: Fault\n";
        current_status = Servo_Status::FAULT;
    }
}

int Kinco::processRecvTPDOFrame(const CanStamped &recv_frame)
{
    memcpy(&status_fbk, recv_frame.Data, 8);
    parseStatusWord(status_fbk.status);
    if(current_status == Servo_Status::FAULT) {
        std::cout << "Servo motor in FAULT status, disable servo motor." << std::endl;
        // SendDisableMotorPDO();
        can->clr_send_buffer(can_ch);

        can->COE().read(can_ch, slave_id, 0x1001, 0);

        // clear error bit
        can->COE().write<uint8_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x80);

        SendDisableMotorPDO();

        exit(-1);
    }
}

int Kinco::processRecvEmcyFrame(const CanStamped &recv_frame)
{
    std::cout << "Received EMCY frame." << std::endl;
    _error_code = recv_frame.Data[0] | (recv_frame.Data[1] << 8);
    uint8_t error_register = recv_frame.Data[2];
    uint8_t error_ext = recv_frame.Data[3]; // 假设扩展错误状态在 Data[3]

    std::cout << std::hex << "error code is:0x" <<_error_code 
                << " error register is:0x"<< static_cast<int>(error_register) <<std::endl;
     
    auto it = emcy_error_map.find(_error_code);
    if (it != emcy_error_map.end()) {
        std::cout << "EMCY Error: " << it->second << std::endl;
    } else {
        std::cout << "EMCY Error: Unknown error code" << std::endl;
    }

    // SendDisableMotorPDO();

    return 0;
}

void Kinco::SendSyncFrame()
{
    CanStamped send_frame;
    send_frame.ID = 0x80;
    send_frame.DataLen = 1;
    send_frame.Data[0] = 0x00;
    can->Write(can_ch, send_frame);
    can->send_msg(can_ch);
    usleep(10000);
    can->recv_msg(can_ch);
}

void Kinco::SendSetPosModeSDO()
{
    can->COE().write<uint16_t>(can_ch, slave_id, 0x6040, 0, 0x0F); // Enable Operation
    can->COE().write<uint8_t>(can_ch, slave_id, 0x6060, 0, 1);
}

void Kinco::SendSetHomeModeSDO()
{
    can->COE().write<uint16_t>(can_ch, slave_id, 0x6040, 0, 0x0F); // Enable Operation
    can->COE().write<uint8_t>(can_ch, slave_id, 0x6060, 0, 6);
}

float Kinco::GetCurPosPDO()
{
    SendSyncFrame();
    return static_cast<float>(status_fbk.pos);
}

Servo_Status Kinco::GetCurStatusPDO()
{
    SendSyncFrame();
    usleep(10000);
    return current_status;
}

int16_t Kinco::GetCurrentPDO(float &ap, float &rms)
{
    SendSyncFrame();
    usleep(10000);
    
    ap = static_cast<float> (status_fbk.current / DEC_TO_AP_DIV);
    rms = static_cast<float> (status_fbk.current / DEC_TO_RMS_DIV);
    return status_fbk.current;
}

MotorStatusFbk Kinco::GetCurFBKPDO(float &pos, float &ap, float &rms)
{
    SendSyncFrame();
    usleep(10000);
    // std::cout << "status_fbk.current:" << status_fbk.current << std::endl;

    pos = static_cast<float>(status_fbk.pos);
    ap = static_cast<float>(status_fbk.current / DEC_TO_AP_DIV);
    rms = static_cast<float>(status_fbk.current / DEC_TO_RMS_DIV);
    return status_fbk;
}
